﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Drawing;
using System.Text;
using FarseerPhysics;
using FarseerPhysics.Collision;
using FarseerPhysics.Collision.Shapes;
using FarseerPhysics.Common;
using FarseerPhysics.Controllers;
using FarseerPhysics.Dynamics;
using FarseerPhysics.Dynamics.Contacts;
using FarseerPhysics.Dynamics.Joints;
using Microsoft.Xna.Framework;
using SFML.Graphics;
using SFML.Window;
using Theta;
using SShape = SFML.Graphics.Shape;
using Color = SFML.Graphics.Color;

namespace Theta.Physics
{
    public class ThetaDebugView : DebugView
    {
        RenderWindow _rd;
        public Color DefaultShapeColor;
        public Color InactiveShapeColor;
        public Color KinematicShapeColor;
        public Color SleepingShapeColor;
        public Color StaticShapeColor;
        public Color TextColor = Color.White;
        private Vector2[] _tempVertices = new Vector2[Settings.MaxPolygonVertices];
        public Vector2 DebugPanelPosition = new Vector2(40, 40);

        private int _max;
        private int _avg;
        private int _min;

        //Performance graph
        public bool AdaptiveLimits = true;
        public int ValuesToGraph = 500;
        public int MinimumValue;
        public int MaximumValue = 1000;
        private List<float> _graphValues = new List<float>();
        public Rectangle PerformancePanelBounds = new Rectangle(250, 40, 200, 100);

        private Vector2[] _background = new Vector2[4];
        public bool Enabled = true;

        public ThetaDebugView(World world) : base(world)
        {
            DefaultShapeColor = To255BasedColor(0.9f, 0.7f, 0.7f);
            InactiveShapeColor = To255BasedColor(0.5f, 0.5f, 0.3f);
            KinematicShapeColor = To255BasedColor(0.5f, 0.5f, 0.9f);
            SleepingShapeColor = To255BasedColor(0.6f, 0.6f, 0.6f);
            StaticShapeColor = To255BasedColor(0.5f, 0.9f, 0.5f);

            DefaultShapeColor.A = 127;
            InactiveShapeColor.A = 127;
            KinematicShapeColor.A = 127;
            SleepingShapeColor.A = 127;
            StaticShapeColor.A = 127;

            AppendFlags(DebugViewFlags.Shape);
        }

        public void Draw(RenderWindow r)
        {
            _rd = r;
            if((Flags & DebugViewFlags.Shape) == DebugViewFlags.Shape)
            {
                foreach(Body b in World.BodyList)
                {
                    Transform xf;
                    b.GetTransform(out xf);
                    foreach (Fixture f in b.FixtureList)
                    {
                        if(b.Enabled == false)
                        {
                            DrawShape(f, xf, InactiveShapeColor);
                        }
                        else if(b.BodyType == BodyType.Static)
                        {
                            DrawShape(f, xf, StaticShapeColor);
                        }
                        else if(b.BodyType == BodyType.Kinematic)
                        {
                            DrawShape(f, xf, KinematicShapeColor);
                        }
                        else if (b.Awake == false)
                        {
                            DrawShape(f, xf, SleepingShapeColor);
                        }
                        else
                        {
                            DrawShape(f, xf, DefaultShapeColor);
                        }
                    }
                }
            }
            if ((Flags & DebugViewFlags.ContactPoints) == DebugViewFlags.ContactPoints)
            {
                //To draw this, we must listen to the PreSolve event, once everything else is in place I'll implement this.
                throw new NotImplementedException();
            }
            if ((Flags & DebugViewFlags.PolygonPoints) == DebugViewFlags.PolygonPoints)
            {
                foreach (Body body in World.BodyList)
                {
                    foreach (Fixture f in body.FixtureList)
                    {
                        PolygonShape polygon = f.Shape as PolygonShape;
                        if (polygon != null)
                        {
                            Transform xf;
                            body.GetTransform(out xf);

                            for (int i = 0; i < polygon.Vertices.Count; i++)
                            {
                                Vector2 tmp = MathUtils.Multiply(ref xf, polygon.Vertices[i]);
                                DrawPoint(tmp, 0.05f, Color.Red);
                            }
                        }
                    }
                }
            }
            if ((Flags & DebugViewFlags.Joint) == DebugViewFlags.Joint)
            {
                foreach (Joint j in World.JointList)
                {
                    DrawJoint(j);
                }
            }
            if ((Flags & DebugViewFlags.Pair) == DebugViewFlags.Pair)
            {
                Color color = To255BasedColor(0.3f, 0.9f, 0.9f);
                for (int i = 0; i < World.ContactManager.ContactList.Count; i++)
                {
                    Contact c = World.ContactManager.ContactList[i];
                    Fixture fixtureA = c.FixtureA;
                    Fixture fixtureB = c.FixtureB;

                    AABB aabbA;
                    fixtureA.GetAABB(out aabbA, 0);
                    AABB aabbB;
                    fixtureB.GetAABB(out aabbB, 0);

                    Vector2 cA = aabbA.Center;
                    Vector2 cB = aabbB.Center;

                    DrawSegment(cA, cB, color);
                }
            }
            if ((Flags & DebugViewFlags.AABB) == DebugViewFlags.AABB)
            {
                Color color = To255BasedColor(0.9f, 0.3f, 0.9f);
                IBroadPhase bp = World.ContactManager.BroadPhase;

                foreach (Body b in World.BodyList)
                {
                    if (b.Enabled == false)
                    {
                        continue;
                    }

                    foreach (Fixture f in b.FixtureList)
                    {
                        for (int t = 0; t < f.ProxyCount; ++t)
                        {
                            FixtureProxy proxy = f.Proxies[t];
                            AABB aabb;
                            bp.GetFatAABB(proxy.ProxyId, out aabb);

                            DrawAABB(ref aabb, color);
                        }
                    }
                }
            }
            if ((Flags & DebugViewFlags.CenterOfMass) == DebugViewFlags.CenterOfMass)
            {
                foreach (Body b in World.BodyList)
                {
                    Transform xf;
                    b.GetTransform(out xf);
                    xf.Position = b.WorldCenter;
                    DrawTransform(ref xf);
                }
            }
            if ((Flags & DebugViewFlags.Controllers) == DebugViewFlags.Controllers)
            {
                for (int i = 0; i < World.ControllerList.Count; i++)
                {
                    Controller controller = World.ControllerList[i];

                    BuoyancyController buoyancy = controller as BuoyancyController;
                    if (buoyancy != null)
                    {
                        AABB container = buoyancy.Container;
                        //                      Lightblue
                        DrawAABB(ref container, new Color(173, 216, 230, 255));
                    }
                }
            }
            if ((Flags & DebugViewFlags.DebugPanel) == DebugViewFlags.DebugPanel)
            {
                DrawDebugPanel();
            }
            if ((Flags & DebugViewFlags.PerformanceGraph) == DebugViewFlags.PerformanceGraph)
            {
                DrawPerformanceGraph();
            }
        }

        private void DrawPerformanceGraph()
        {
            _graphValues.Add(World.UpdateTime);

            if (_graphValues.Count > ValuesToGraph + 1)
                _graphValues.RemoveAt(0);

            float x = PerformancePanelBounds.X;
            float deltaX = PerformancePanelBounds.Width / (float)ValuesToGraph;
            float yScale = PerformancePanelBounds.Bottom - (float)PerformancePanelBounds.Top;

            // we must have at least 2 values to start rendering
            if (_graphValues.Count > 2)
            {
                _max = (int)_graphValues.Max();
                _avg = (int)_graphValues.Average();
                _min = (int)_graphValues.Min();

                if (AdaptiveLimits)
                {
                    MaximumValue = _max;
                    MinimumValue = 0;
                }

                //Draw background.
                _background[0] = Converter.ToMeters(new Vector2(PerformancePanelBounds.X, PerformancePanelBounds.Y));
                _background[1] = Converter.ToMeters(new Vector2(PerformancePanelBounds.X,
                                             PerformancePanelBounds.Y + PerformancePanelBounds.Height));
                _background[2] = Converter.ToMeters(new Vector2(PerformancePanelBounds.X + PerformancePanelBounds.Width,
                                             PerformancePanelBounds.Y + PerformancePanelBounds.Height));
                _background[3] = Converter.ToMeters(new Vector2(PerformancePanelBounds.X + PerformancePanelBounds.Width,
                                             PerformancePanelBounds.Y));

                DrawSolidPolygon(_background, 4, new Color(169, 169, 169, 100));

                // start at last value (newest value added)
                // continue until no values are left
                for (int i = _graphValues.Count - 1; i > 0; i--)
                {
                    float y1 = PerformancePanelBounds.Bottom -
                               ((_graphValues[i] / (MaximumValue - MinimumValue)) * yScale);
                    float y2 = PerformancePanelBounds.Bottom -
                               ((_graphValues[i - 1] / (MaximumValue - MinimumValue)) * yScale);

                    Vector2 x1 =
                        new Vector2(MathHelper.Clamp(x, PerformancePanelBounds.Left, PerformancePanelBounds.Right),
                                    MathHelper.Clamp(y1, PerformancePanelBounds.Top, PerformancePanelBounds.Bottom));

                    Vector2 x2 =
                        new Vector2(
                            MathHelper.Clamp(x + deltaX, PerformancePanelBounds.Left, PerformancePanelBounds.Right),
                            MathHelper.Clamp(y2, PerformancePanelBounds.Top, PerformancePanelBounds.Bottom));

                    DrawSegment(Converter.ToMeters(x1), Converter.ToMeters(x2), new Color(144, 238, 144, 255));

                    x += deltaX;
                }
            }

            Point center = new Point(PerformancePanelBounds.X + (PerformancePanelBounds.Width / 2), PerformancePanelBounds.Y + (PerformancePanelBounds.Height / 2));

            DrawString(PerformancePanelBounds.Right + 10, PerformancePanelBounds.Top, "Max: " + _max);
            DrawString(PerformancePanelBounds.Right + 10, center.Y - 7, "Avg: " + _avg);
            DrawString(PerformancePanelBounds.Right + 10, PerformancePanelBounds.Bottom - 15, "Min: " + _min);
        }

        public void DrawDebugPanel()
        {
            int fixtures = 0;
            for (int i = 0; i < World.BodyList.Count; i++)
            {
                fixtures += World.BodyList[i].FixtureList.Count;
            }

            int x = (int)DebugPanelPosition.X;
            int y = (int)DebugPanelPosition.Y;

            DrawString(x, y, "Objects:" +
                             "\n- Bodies: " + World.BodyList.Count +
                             "\n- Fixtures: " + fixtures +
                             "\n- Contacts: " + World.ContactList.Count +
                             "\n- Joints: " + World.JointList.Count +
                             "\n- Controllers: " + World.ControllerList.Count +
                             "\n- Proxies: " + World.ProxyCount);

            DrawString(x + 110, y, "Update time:" +
                                   "\n- Body: " + World.SolveUpdateTime +
                                   "\n- Contact: " + World.ContactsUpdateTime +
                                   "\n- CCD: " + World.ContinuousPhysicsTime +
                                   "\n- Joint: " + World.Island.JointUpdateTime +
                                   "\n- Controller: " + World.ControllersUpdateTime +
                                   "\n- Total: " + World.UpdateTime);
        }

        public void DrawString(int x, int y, string s)
        {
            Text t = new Text(s);
            t.CharacterSize = 12;
            t.Position = new Vector2f(x, y);
            _rd.Draw(t);
        }

        void DrawJoint(Joint joint)
        {
            if(!joint.Enabled)
                return;

            Body b1 = joint.BodyA;
            Body b2 = joint.BodyB;
            Transform xf1, xf2;
            b1.GetTransform(out xf1);

            Vector2 x2 = Vector2.Zero;

            // WIP David
            if(!joint.IsFixedType())
            {
                b2.GetTransform(out xf2);
                x2 = xf2.Position;
            }

            Vector2 p1 = joint.WorldAnchorA;
            Vector2 p2 = joint.WorldAnchorB;
            Vector2 x1 = xf1.Position;

            Color color = To255BasedColor(0.5f, 0.8f, 0.8f);

            switch(joint.JointType)
            {
                case JointType.Distance:
                    DrawSegment(p1, p2, color);
                    break;
                case JointType.Pulley:
                    PulleyJoint pulley = (PulleyJoint)joint;
                    Vector2 s1 = pulley.GroundAnchorA;
                    Vector2 s2 = pulley.GroundAnchorB;
                    DrawSegment(s1, p1, color);
                    DrawSegment(s2, p2, color);
                    DrawSegment(s1, s2, color);
                    break;
                case JointType.FixedMouse:
                    DrawPoint(p1, 0.5f, To255BasedColor(0.0f, 1.0f, 0.0f));
                    DrawSegment(p1, p2, To255BasedColor(0.8f, 0.8f, 0.8f));
                    break;
                case JointType.Revolute:
                    //DrawSegment(x2, p1, color);
                    DrawSegment(p2, p1, color);
                    DrawSolidCircle(p2, 0.1f, Vector2.Zero, Color.Red);
                    DrawSolidCircle(p1, 0.1f, Vector2.Zero, Color.Blue);
                    break;
                case JointType.FixedAngle:
                    //Should not draw anything.
                    break;
                case JointType.FixedRevolute:
                    DrawSegment(x1, p1, color);
                    DrawSolidCircle(p1, 0.1f, Vector2.Zero, new Color(255, 192, 203, 255));
                    break;
                case JointType.FixedLine:
                    DrawSegment(x1, p1, color);
                    DrawSegment(p1, p2, color);
                    break;
                case JointType.FixedDistance:
                    DrawSegment(x1, p1, color);
                    DrawSegment(p1, p2, color);
                    break;
                case JointType.FixedPrismatic:
                    DrawSegment(x1, p1, color);
                    DrawSegment(p1, p2, color);
                    break;
                case JointType.Gear:
                    DrawSegment(x1, x2, color);
                    break;
                    //case JointType.Weld:
                    //    break;
                default:
                    DrawSegment(x1, p1, color);
                    DrawSegment(p1, p2, color);
                    DrawSegment(x2, p2, color);
                    break;
            }
        }

        public void DrawPoint(Vector2 p, float size, Color color)
        {
            Vector2[] verts = new Vector2[4];
            float hs = size / 2.0f;
            verts[0] = p + new Vector2(-hs, -hs);
            verts[1] = p + new Vector2(hs, -hs);
            verts[2] = p + new Vector2(hs, hs);
            verts[3] = p + new Vector2(-hs, hs);

            DrawSolidPolygon(verts, 4, color);
        }

        public byte To255ColorValue(float value)
        {
            float f2 = Math.Max(0.0f, Math.Min(1.0f, value));
            return (f2 == 1.0f) ? (byte)255 : (byte)Math.Floor(f2 * 256.0);
        }

        public Color To255BasedColor(float red, float blue, float green)
        {
            return new Color(To255ColorValue(red), To255ColorValue(blue), To255ColorValue(green));
        }

        public void DrawAABB(ref AABB aabb, Color color)
        {
            Vector2[] verts = new Vector2[4];
            verts[0] = new Vector2(aabb.LowerBound.X, aabb.LowerBound.Y);
            verts[1] = new Vector2(aabb.UpperBound.X, aabb.LowerBound.Y);
            verts[2] = new Vector2(aabb.UpperBound.X, aabb.UpperBound.Y);
            verts[3] = new Vector2(aabb.LowerBound.X, aabb.UpperBound.Y);

            DrawPolygon(verts, 4, color);
        }

        public void DrawShape(Fixture fixture, Transform xf, Color color)
        {
            switch (fixture.ShapeType)
            {
                case ShapeType.Circle:
                    {
                        CircleShape circle = (CircleShape)fixture.Shape;
                        Vector2 center = MathUtils.Multiply(ref xf, circle.Position);
                        float radius = circle.Radius;
                        Vector2 axis = xf.R.Col1;

                        DrawSolidCircle(center, radius, axis, color);
                        break;
                    }
                case ShapeType.Edge:
                    {
                        EdgeShape edge = (EdgeShape)fixture.Shape;
                        Vector2 v1 = MathUtils.Multiply(ref xf, edge.Vertex1);
                        Vector2 v2 = MathUtils.Multiply(ref xf, edge.Vertex2);
                        DrawSegment(v1, v2, color);
                        break;
                    }
                case ShapeType.Polygon:
                    {
                        PolygonShape poly = (PolygonShape)fixture.Shape;
                        int vertexCount = poly.Vertices.Count;
                        Debug.Assert(vertexCount <= Settings.MaxPolygonVertices);

                        for(int i = 0; i < vertexCount; ++i)
                        {
                            _tempVertices[i] = MathUtils.Multiply(ref xf, poly.Vertices[i]);
                        }

                        DrawSolidPolygon(_tempVertices, vertexCount, color);
                        break;
                    }
                case ShapeType.Loop:
                    {
                        LoopShape loop = (LoopShape)fixture.Shape;
                        int count = loop.Vertices.Count;

                        Vector2 v1 = MathUtils.Multiply(ref xf, loop.Vertices[count - 1]);
                        DrawCircle(v1, 0.05f, color);
                        for(int i = 0; i < count; ++i)
                        {
                            Vector2 v2 = MathUtils.Multiply(ref xf, loop.Vertices[i]);
                            DrawSegment(v1, v2, color);
                            v1 = v2;
                        }
                        break;
                    }
            }
        }

        public override void DrawPolygon(Vector2[] vertices, int count, float red, float blue, float green)
        {
            DrawPolygon(vertices, count, To255BasedColor(red, blue, green));
        }

        public void DrawPolygon(Vector2[] vertices, int count, Color c)
        {
            SShape polygon = new SShape();
            Color outlineColor = c;
            outlineColor.A = 255;
            for(int i = 0; i < count; i++)
            {
                polygon.AddPoint(Converter.ToPixels(Converter.ToVector2f(vertices[i])), c, outlineColor);
            }

            polygon.EnableFill(false);
            polygon.EnableOutline(true);
            polygon.OutlineThickness = 1;
            _rd.Draw(polygon);
        }

        public void DrawSolidPolygon(Vector2[] vertices, int count, Color c)
        {
            SShape polygon = new SShape();
            Color outlineColor = new Color(c);
            outlineColor.A = 255;
            for (int i = 0; i < count; i++)
            {
                polygon.AddPoint(Converter.ToPixels(Converter.ToVector2f(vertices[i])), c, outlineColor);
            }
            polygon.EnableFill(true);
            polygon.EnableOutline(true);
            polygon.OutlineThickness = 1;
            _rd.Draw(polygon);
        }

        public override void DrawSolidPolygon(Vector2[] vertices, int count, float red, float blue, float green)
        {
            DrawSolidPolygon(vertices, count, To255BasedColor(red, blue, green));
        }

        public void DrawCircle(Vector2 center, float radius, Color color)
        {
            Color outlineColor = color;
            outlineColor.A = 255;
            SShape s = SShape.Circle(Converter.ToPixels(Converter.ToVector2f(center)), Converter.ToPixels(radius), color, 1f, outlineColor);
            //s.Origin = Converter.ToVector2f(axis);
            s.EnableFill(false);
            s.EnableOutline(true);

            _rd.Draw(s);
        }

        public override void DrawCircle(Vector2 center, float radius, float red, float blue, float green)
        {
            DrawCircle(center, radius, To255BasedColor(red, blue, green));
        }

        public override void DrawSolidCircle(Vector2 center, float radius, Vector2 axis, float red, float blue, float green)
        {
            DrawSolidCircle(center, radius, axis, To255BasedColor(red, blue, green));
        }

        public void DrawSolidCircle(Vector2 center, float radius, Vector2 axis, Color color)
        {
            Color outlineColor = color;
            outlineColor.A = 255;
            SShape s = SShape.Circle(Converter.ToPixels(Converter.ToVector2f(center)), Converter.ToPixels(radius), color, 1f, outlineColor);
            //s.Origin = Converter.ToVector2f(axis);
            s.EnableFill(true);
            s.EnableOutline(true);
            
            _rd.Draw(s);
        }

        public void DrawSegment(Vector2 start, Vector2 end, Color c)
        {
            SShape s = SShape.Line(Converter.ToVector2f(Converter.ToPixels(start)), Converter.ToVector2f(Converter.ToPixels(end)), 1, c);
            _rd.Draw(s);
        }

        public override void DrawSegment(Vector2 start, Vector2 end, float red, float blue, float green)
        {
            DrawSegment(start, end, To255BasedColor(red, blue, green));
        }

        public override void DrawTransform(ref Transform transform)
        {
            const float axisScale = 0.4f;
            Vector2 p1 = transform.Position;

            Vector2 p2 = p1 + axisScale * transform.R.Col1;
            DrawSegment(p1, p2, Color.Red);

            p2 = p1 + axisScale * transform.R.Col2;
            DrawSegment(p1, p2, Color.Green);
        }
    }
}
